#include "PowerSpectrum.h"
#include "MoreFunctional.h"
#include "/home/mjarvis/misc/nr/zbrent.cpp"

#include <math.h>
#include "CalcT.h"
#include <iostream>
using std::endl;
using std::ptr_fun;
#include <fstream>
using std::ifstream;
#include <map>
using std::multimap;
using std::map;
#include <vector>
using std::vector;

#include "Int.h"
using integ::int1d;
using integ::IntRegion;

#include "dbg.h"

//#define CFRS
//#define CRS
#define DES
//#define SINGLEZB 1.0


//#define PD

//#define SCH_U

//#define ALLINT

const double PI = 3.141592653589793;
const double TWOPI = 2.0*PI;
const double TWOPISQ = TWOPI*PI;

template <class T> 
static T SQR(const T& x) { return x*x; }

extern "C" { // Derek's code
    typedef double D;
    void def_universe_(
        const D& om_m, const D& om_v, const D& om_b,
        const D& h, const D& A_s, const D& n, const D& w, const D& wa);
    D smithp3d_(const D& k, const D& z);
    D linearp3d_(const D& k, const D& z);
    D pdp3d_(const D& k, const D& z);
}

const double bgmmin = 19.;
const double bgmmax = 23.;
const double fgmmin = 18.5;
const double fgmmax = 20.0;

const char* crsfile = "caltech.cousins.zp1";
const char* cfrsfile = "CFRS.extinct.zp1";
const char* magweightfile = "magweights";

const char* desfile = "des.dc5.z";

#ifdef SCH_U

double WMap(double eta)
{
    // For Schneider U, W(lR) = U~_R(l)^2 = (24/(lR)^2 J_4(lR))^2
    double temp = 24./eta/eta*jn(4,eta);
    return temp*temp;
}

#else 

double WMap(double eta)
{
    // For Crittenden U, W(lR) = U~_R(l)^2 = (lR)^4/4 exp(-(lR)^2)
    double etasq = eta*eta;
    return etasq*etasq/4.*exp(-etasq);
}

#endif

double WVar(double eta)
{
    // WVar(lR) = 4 J_1(lR)^2/(lR)^2
    double temp = 2. * j1(eta) / eta;
    return temp*temp;
}

void ReadP(multimap<double,double>& pmap, const Universe& u,
           double mmin, double mmax, bool fg)
{
    static bool first = true;

#ifdef DES
    static std::vector<double> z;
    static std::vector<double> wt;
    if (first) {
        dbg<<"Using DES redshifts\n"; 
        std::vector<double> zvector;
        dbg<<"Reading DES file: "<<desfile<<std::endl;
        ifstream fin(desfile);
        if (!fin) myerror("Unable to open ",desfile);
        double ra,dec,z1;
        double maxz = 0.;
        while (fin >> ra >> dec >> z1) {
            zvector.push_back(z1);
            if (z1 > maxz) maxz = z1;
        }
        dbg<<"Done reading "<<desfile<<": N = "<<zvector.size()<<std::endl;
        // Rebin in 0.01 bins in z.
        int n = int(floor(maxz*100.)) + 1;
        z.resize(n);
        wt.resize(n);
        double sumw;
        for(size_t i=0;i<zvector.size();++i) {
            double z1 = zvector[i];
            int k = int(floor(z1*100.));
            z[k] += z1;
            wt[k] += 1.0;
            sumw += 1.0;
        }
        for(size_t k=0;k<z.size();++k) {
            if (wt[k] > 0.) z[k] /= wt[k];
            else z[k] = (k+0.5)/100.;
            wt[k] /= sumw;
        }
        dbg<<"z  wt\n";
        for(size_t k=0;k<z.size();++k) {
            dbg<<z[k]<<"  "<<wt[k]<<std::endl;
        }
    }
    for (size_t k=0;k<z.size();++k)
        pmap.insert(std::make_pair(u.w_z(z[k]),wt[k]));
    xdbg<<"Made pmap: N = "<<pmap.size()<<std::endl;
#else
    // Read Caltech Redshift Survey data
    double m,z;
    vector<double> mvector;
    vector<double> zvector;

    // If foreground, then use both, otherwise default is CRS only
#ifndef CRS
    if (fg) {
#endif
        if (first) { dbg<<"Using CRS redshifts\n"; }
        ifstream fin(crsfile);
        if (!fin) myerror("Unable to open ",crsfile);
        while (fin >> m >> z) if (m >= mmin && m <= mmax) {
            mvector.push_back(m);
            if (z < 0.) z = 1.; // Use method 1 from JB02 for unknown redshifts
            zvector.push_back(z);
        }
#ifndef CRS
    }
#endif

#ifndef CFRS
    if (fg) { // Then also use Canada-France Redshift Survey
#endif
        if (first) { dbg<<"Using CFRS redshifts\n"; }
        ifstream fin2(cfrsfile);
        if (!fin2) myerror("Unable to open ",cfrsfile);
        while (fin2 >> m >> z) if (m >= mmin && m <= mmax) {
            mvector.push_back(m);
            if (z < 0.) z = 1.; // Use method 1 from JB02 for unknown redshifts
            zvector.push_back(z);
        }
#ifndef CFRS
    }
#endif

    Assert(mvector.size() > 0);

    // Read observed weights as function of mag
    ifstream fin3(magweightfile);
    if (!fin3) myerror("unable to open ",magweightfile);
    vector<double> obsmvector;
    vector<double> obswtvector;
    double n,w;
    while (fin3 >> m >> w >> n) {
        obsmvector.push_back(m);
        obswtvector.push_back(w);
    }

    // Determine Wi for each reshift survey galaxy
    vector<double> wtvector(zvector.size());
    vector<int> jvector(zvector.size());
    vector<int> countvector(obsmvector.size());

    for(size_t i=0;i<zvector.size();i++) {
        if (mvector[i] < obsmvector[0]) 
            myerror("redshift survey mag is less than minimum mag in weight file");
        int j=obsmvector.size()-1;
        while (mvector[i] < obsmvector[j]) --j;
        jvector[i] = j;
        countvector[j]++;
    }
    double sumw = 0.;
    for(size_t i=0;i<zvector.size();i++) {
        int j = jvector[i];
        wtvector[i] = obswtvector[j]/countvector[j];
        sumw += wtvector[i];
    }
    for(size_t i=0;i<zvector.size();i++) {
        wtvector[i] /= sumw;
        pmap.insert(std::make_pair(u.w_z(zvector[i]),wtvector[i]));
    }
#endif

    first = false;
}

double rawg(const Universe& u, double w) 
    // lensing strength of lens at distance w
{
#ifdef SINGLEZB
    const double w0 = u.w_z(SINGLEZB);
    if (w < w0) return (w0-w)/w0;
    else return 0.;
#else
    typedef multimap<double,double>::const_iterator mapit;

    double sum=0.;
    // multimap stores values sorted by key (w)
    // so this loops over all w' >= w
    if (u.pbmap.size() == 0) ReadP(u.pbmap,u,bgmmin,bgmmax,false);
    for(mapit it=u.pbmap.lower_bound(w);it != u.pbmap.end(); it++) {
        const double wprime = it->first;
        const double wt = it->second;
        sum += wt * u.fk(wprime - w) / u.fk(wprime);
    }
    return sum;
#endif
}

double unnormpf(const Universe& u, double w)
{
    Assert(w>=0.);
    const double w0 = u.w_z(1.);
    const double factor = 1.5*u.om_m/u.fk(w0);
    if (w < w0) return factor * u.fk(w)*u.fk(w0-w)*(1.+u.z_w(w));
    else return 0.;
}

double rawpf(const Universe& u, double w) 
    // smoothed estimate of foreground galaxy density 
    // pfsmooth = int_w-dw,w+dw pf(w) dw
{
#ifdef SINGLEZB
    if (u.pfint == 0.) {
        const double wmax = u.w_z(SINGLEZB);
        IntRegion<double> intreg(0.,wmax);
        u.pfint = int1d(bind21(ptr_fun(unnormpf),u),intreg,1.e-15,
                        u.precise ? 1.e-4 : 1.e-2);
        dbg <<"kappa_min = "<<-u.pfint<<endl;
    }
    return unnormpf(u,w)/u.pfint;
#else
    const double deltaw = 0.05;
    const double twodeltaw = 2.*deltaw;

    typedef multimap<double,double>::const_iterator mapit;

    if (u.pfmap.size()==0) ReadP(u.pfmap,u,fgmmin,fgmmax,true);
    double sum = 0.;
    for(mapit it=u.pfmap.lower_bound(w-deltaw);it != u.pfmap.upper_bound(w+deltaw); it++) {
        //const double wprime = it->first;
        const double wt = it->second;
        sum += wt;
    }
    sum /= twodeltaw;
    return sum;
#endif
}

double unnormpb(const Universe& u, double w)
{
    Assert(w>=0.);
    const double w0 = u.w_z(1.);
    const double factor = 1.5*u.om_m/u.fk(w0);
    if (w < w0) return factor * u.fk(w)*u.fk(w0-w)*(1.+u.z_w(w));
    else return 0.;
}

double rawpb(const Universe& u, double w) 
    // smoothed estimate of background galaxy density 
    // pbsmooth = int_w-dw,w+dw pb(w) dw
{
#ifdef SINGLEZB
    if (u.pbint == 0.) {
        const double wmax = u.w_z(SINGLEZB);
        IntRegion<double> intreg(0.,wmax);
        u.pbint = int1d(bind21(ptr_fun(unnormpb),u),intreg,1.e-15,
                        u.precise ? 1.e-4 : 1.e-2);
        dbg <<"kappa_min = "<<-u.pbint<<endl;
    }
    return unnormpb(u,w)/u.pbint;
#else
    const double deltaw = 0.05;
    const double twodeltaw = 2.*deltaw;

    typedef multimap<double,double>::const_iterator mapit;

    if (u.pbmap.size()==0) ReadP(u.pbmap,u,bgmmin,bgmmax,false);
    double sum = 0.;
    for(mapit it=u.pbmap.lower_bound(w-deltaw);it != u.pbmap.upper_bound(w+deltaw); it++) {
        //const double wprime = it->first;
        const double wt = it->second;
        sum += wt;
    }
    sum /= twodeltaw;
    return sum;
#endif
}

#ifdef SINGLEZB
    // Only use saved versions if these are complicated 
    // (ie. if there is a range of background redshifts)
    double Universe::g(double w) const { return rawg(*this,w); }
    double Universe::pf(double w) const { return rawpf(*this,w); }
    double Universe::pb(double w) const { return rawpb(*this,w); }
#else 
    double Universe::g(double w) const { return sg(w); }
    // lensing strength of lens at distance w
    double Universe::pf(double w) const { return spf(w); }
    // smoothed estimate of foreground galaxy density 
    // pfsmooth = int_w-dw,w+dw pf(w) dw
    double Universe::pb(double w) const { return spb(w); }
    // smoothed estimate of background galaxy density 
    // pfsmooth = int_w-dw,w+dw pf(w) dw
#endif

double calc_hsq(double om_m, double om_v, double w, double wa, double a)
{
    // H^2 = H0^2 * [ Om_m a^-3 + Om_k a^-2 + 
    //                Om_DE exp (-3 [ (1+w+wa)*lna + wa*(1-a) ] ) ]
    // Ignore the H0^2 scaling
    double lna = log(a);
    double hsq = om_m * exp(-3.*lna);
    double om_k = 1.-om_m-om_v;
    if (om_k != 0.) hsq += om_k * exp(-2.*lna);
    if (wa == 0.) 
        if (w == -1.)
            hsq += om_v;
        else
            hsq += om_v * exp(-3.*(1.+w)*lna);
    else
        hsq += om_v * exp(-3.*( (1.+w+wa)*lna + wa*(1.-a) ) );
    return hsq;
}

namespace dh {

    double om_m, om_v, w, wa;

    double dhsq_da(double a)
    {
        // H^2 = Om_m a^-3 + Om_k a^-2 + 
        //       Om_DE exp (-3 [ (1+w+wa)*lna + wa*(1-a) ] ) 
        // dH^2/da = -3 Om_m a^-4 - 2 Om_k a^-3 + 
        //           -3 Om_DE exp (-3 [ (1+w+wa)*lna + wa*(1-a) ] ) *
        //              ( (1+w+wa)/a - wa)
        // Since I haven't updated zbrent to use function objects, it still
        // want a function of type: double *f(double).
        // So use globu to store the u info.

        double lna = log(a);
        double dhsq = -3.*om_m * exp(-4.*lna);
        double om_k = 1.-om_m-om_v;
        if (om_k != 0.) dhsq -= 2.*om_k * exp(-3.*lna);
        if (wa == 0.) {
            if (w != -1.)
                dhsq -= 3.*om_v * exp(-3.*(1.+w)*lna) * (1.+w)/a;
            // else nothing
        }
        else
            dhsq -= 3.*om_v * exp(-3.*( (1.+w+wa)*lna + wa*(1.-a) ) ) *
                ( (1.+w+wa)/a - wa);
        return dhsq;
    }

} // dh namespace

bool NegHsq(double om_m, double om_v, double w, double wa)
{
    dh::om_m = om_m;
    dh::om_v = om_v;
    dh::w = w;
    dh::wa = wa;

    // At a = 0, dhsq/da is always < 0
    // So only a minimum if dhsq/da(a) > 0 for some a
    // In practice, just check a = 1.0, 0.9, 0.7
    double a1=1.0;
    double dhsq_a1 = dh::dhsq_da(1.0);
    if (dhsq_a1 <= 0.) {
        a1 = 0.9;
        dhsq_a1 = dh::dhsq_da(0.9);
        if (dhsq_a1 <= 0.) {
            a1 = 0.7;
            dhsq_a1 = dh::dhsq_da(0.7);
            if (dhsq_a1 <= 0.) { 
                return false;
            }
        }
    }

    //dbg<<"Neg for "<<om_m<<", "<<om_v<<", "<<w<<", "<<wa<<"?\n";
    double amin = zbrent(dh::dhsq_da,1.e-5,a1,1.e-3);
    //dbg<<"  amin at "<<amin;
    double hsq_amin = calc_hsq(om_m,om_v,w,wa,amin);
    if (abs(hsq_amin) < 0.01) {
        //dbg<<"  ---  hsq = "<<hsq_amin<<" is too close: redo:\n";
        amin = zbrent(dh::dhsq_da,1.e-5,1.,1.e-6);
        //dbg<<"  new amin at "<<amin;
        hsq_amin = calc_hsq(om_m,om_v,w,wa,amin);
    }
    //dbg<<" ---  hsq("<<amin<<") = "<<hsq_amin<<endl;
    //double da = 0.001;
    //double hsq1 = calc_hsq(om_m,om_v,w,wa,amin-da);
    //double hsq2 = calc_hsq(om_m,om_v,w,wa,amin+da);
    //dbg<<"nominal dhsq/da = "<<dh::dhsq_da(amin)<<endl;
    //dbg<<"actual dhsq/da = "<<(hsq2-hsq1)/(2*da)<<endl;
    //dbg<<"d^2hsq/da^2 = "<<(hsq2+hsq1-2*hsq_amin)/(da*da)<<endl;
    return hsq_amin <= 0.01;
}

double rawdwda(const Universe& u, double a)
    // Technically, this is -dw/da, since w increase as a decreases.
{
    static const Universe* u_neghsq=0;

    double hsq = calc_hsq(u.om_m,u.om_v,u.w,u.wa,a);
    if (hsq <= 0.) {
        if (&u != u_neghsq) {
            dbg<<"Invalid hsq for a = "<<a<<endl;
            dbg<<"om_m = "<<u.om_m<<endl;
            dbg<<"om_v = "<<u.om_v<<endl;
            dbg<<"om_k = "<<1.-u.om_m-u.om_v<<endl;
            dbg<<"flat = "<<u.flat<<endl;
            dbg<<"w = "<<u.w<<endl;
            dbg<<"wa = "<<u.wa<<endl;
            dbg<<"=> hsq = "<<hsq<<endl;

            dbg<<"NegHsq = "<<NegHsq(u.om_m,u.om_v,u.w,u.wa)<<endl;
            dbg<<"nominal dhsq/da(1) = "<<dh::dhsq_da(1.)<<endl;
            dbg<<"nominal dhsq/da(1.e-5) = "<<dh::dhsq_da(1.e-5)<<endl;
            dbg<<"nominal dhsq/da("<<a<<") = "<<dh::dhsq_da(a)<<endl;
            double da = 0.001;
            double hsq1 = calc_hsq(u.om_m,u.om_v,u.w,u.wa,a-da);
            double hsq2 = calc_hsq(u.om_m,u.om_v,u.w,u.wa,a+da);
            dbg<<"actual dhsq/da = "<<(hsq2-hsq1)/(2*da)<<endl;

            dbg<<"hsq(1.e-5) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,1.e-5)<<endl;
            dbg<<"hsq(0.1) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.1)<<endl;
            dbg<<"hsq(0.2) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.2)<<endl;
            dbg<<"hsq(0.3) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.3)<<endl;
            dbg<<"hsq(0.4) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.4)<<endl;
            dbg<<"hsq(0.5) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.5)<<endl;
            dbg<<"hsq(0.6) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.6)<<endl;
            dbg<<"hsq(0.7) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.7)<<endl;
            dbg<<"hsq(0.8) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.8)<<endl;
            dbg<<"hsq(0.9) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,0.9)<<endl;
            dbg<<"hsq(1.0) = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,1.0)<<endl;
            dbg<<"hsq("<<a<<") = "<<calc_hsq(u.om_m,u.om_v,u.w,u.wa,a)<<endl;

            u_neghsq = &u;
        }
        return 1.e100;
        //abort();
    } else 
        return 1./(sqrt(hsq)*(a*a));
}

double raww_z(const Universe& u, double z) 
    // coordinate distance (in units of c/Ho) as a function of redshift
    // w = int( 1/sqrt(H(z)) dz ) = int( 1/sqrt(H(a)) 1/a^2 da )
    // H(a) = H0 sqrt( Om_m a^-3 + Om_k a^-2 +
    //                 Om_de exp(3 int(1+w(a') dln(a'), a'=a..1) ) )
    // For w(a) = w0 + wa(1-a), we can do the internal integral:
    // ... Om_de exp( -3(1+w0+wa) ln(a) - 3 wa(1-a) )
{
    IntRegion<double> intreg(1./(1.+z),1);
    return int1d(bind21(ptr_fun(rawdwda),u),intreg,1.e-15,
                 u.precise ? 1.e-4 : 1.e-2);
}

struct pomIntegrand : public std::unary_function<double, double>
{
    pomIntegrand(const Universe& _u, double _l) : u(_u),l(_l) {}
    double operator()(double w) const
    { 
        Assert(w>=0.);
        double z = u.z_w(w);
        double fkw = u.fk(w);
        double pfofk = u.pf(w)/fkw;
        return pfofk*pfofk * u.NLP3d(l/fkw/u.coverH,z); 
    }

    const Universe& u;
    double l;
};

double rawpom(const Universe& u, double l)
{
#ifdef ALLINT
    const double wmax = u.w_z(5.);

    IntRegion<double> intreg(0.,wmax);
    pomIntegrand integrand(u,l);
    return int1d(
        integrand,intreg,1.e-15, u.precise ? 1.e-3 : 1.e-1);
#else
    typedef multimap<double,double>::const_iterator mapit;

    if (u.pfmap.size()==0) ReadP(u.pfmap,u,fgmmin,fgmmax,true);
    double sum = 0.;
    for(mapit it = u.pfmap.begin(); it != u.pfmap.end(); it++) {
        const double wprime = it->first;
        Assert(wprime>=0.);
        const double zprime = u.z_w(wprime);
        const double wt = it->second;
        const double fkw = u.fk(wprime);
        sum += wt * u.pf(wprime)/fkw/fkw * u.NLP3d(l/fkw/u.coverH,zprime);
    }
    return sum;
#endif
}

struct fomIntegrand : public std::unary_function<double, double>
{
    fomIntegrand(const Universe& _u, double _R) : u(_u), R(_R) {}
    double operator()(double l) const
    { 
        return l * j0(l*R) * u.pom(l); 
    }

    const Universe& u;
    double R;
};

double rawfom(const Universe& u, double R)
    // omega(R) = fom b^2
    // fom(R) = 1/2Pi int dl l J0(lR) int dw pf(w)^2/fk(w)^2 P3D(l/fk(w);w)
{
    const double factor = 1./u.coverHcb / TWOPI;

    IntRegion<double> intreg(0.001/R,1000./R);
    intreg.AddSplit(0.1/R);
    intreg.AddSplit(10./R);
    fomIntegrand integrand(u,R);
    //dbg <<"before integ for fom("<<R<<")"<<endl;
    double answer = int1d(
        integrand,intreg,1.e-15, u.precise ? 1.e-3 : 1.e-1) * factor;
    //dbg <<"fom("<<R<<")  = "<<answer<<endl;
    return answer;
}

struct pgamIntegrand : public std::unary_function<double, double>
{
    pgamIntegrand(const Universe& _u, double _l) : u(_u), l(_l) {}
    double operator()(double w) const
    {
        Assert(w>=0.);
        double z = u.z_w(w);
        double fkw = u.fk(w);
        return u.pf(w)*u.g(w)*(1.+z)/fkw * u.NLP3d(l/fkw/u.coverH,z); 
    }

    const Universe& u;
    double l;
};

double rawpgam(const Universe& u, double l)
{
#ifdef ALLINT
    const double wmax = u.w_z(5.);

    IntRegion<double> intreg(0.,wmax);
    pgamIntegrand integrand(u,l);
    return int1d(
        integrand,intreg,1.e-15, u.precise ? 1.e-3 : 1.e-1);
#else
    typedef multimap<double,double>::const_iterator mapit;

    if (u.pfmap.size()==0) ReadP(u.pfmap,u,fgmmin,fgmmax,true);
    double sum = 0.;
    for(mapit it = u.pfmap.begin(); it != u.pfmap.end(); it++) {
        const double wprime = it->first;
        Assert(wprime>=0.);
        const double zprime = u.z_w(wprime);
        const double wt = it->second;
        const double fkw = u.fk(wprime);
        sum += wt * u.g(wprime)*(1.+zprime)/fkw * u.NLP3d(l/fkw/u.coverH,zprime);
    }
    return sum;
#endif
}

struct fgamIntegrand : public std::unary_function<double, double>
{
    fgamIntegrand(const Universe& _u, double _R) : u(_u), R(_R) {}
    double operator()(double l) const
    { 
        return l * jn(2,l*R) * u.pgam(l); 
    }

    const Universe& u;
    double R;
};

double rawfgam(const Universe& u, double R)
    // omega(R) = fgam b^2
    // fgam(R) = 1/2Pi int dl l J0(lR) int dw pf(w)^2/fk(w)^2 P3D(l/fk(w);w)
{
    //dbg<<"start rawfgam("<<R<<")"<<endl;
    const double factor = 1.5*u.om_m/u.coverHcb / TWOPI;

    IntRegion<double> intreg(0.001/R,1000./R);
    intreg.AddSplit(0.1/R);
    intreg.AddSplit(10./R);
    fgamIntegrand integrand(u,R);
    //dbg<<"before integ for fgam("<<R<<")"<<endl;
    double answer = int1d(
        integrand,intreg,1.e-15, u.precise ? 1.e-3 : 1.e-1) * factor;
    //dbg<<"fgam("<<R<<")  = "<<answer<<endl;
    return answer;
}

struct pkappaIntegrand : public std::unary_function<double, double>
{
    pkappaIntegrand(const Universe& _u, double _l) : u(_u), l(_l) {}
    double operator()(double w) const
    {
        Assert(w>=0.);
        //dbg<<"w = "<<w<<endl;
        double z = u.z_w(w);
        //dbg<<"z = "<<z<<endl;
        double fkw = u.fk(w);
        //dbg<<"fkw = "<<fkw<<endl;
        double k = l/fkw/u.coverH;
        //dbg<<"l = "<<l<<", w = "<<w<<", k = "<<k<<", z = "<<z<<endl;
        double p3d = u.NLP3d(k,z);
        //dbg<<"p3d("<<k<<','<<z<<") = "<<p3d<<endl;
        return SQR(u.g(w)*(1.+z)) * p3d;
    }

    const Universe& u;
    double l;
};

double rawpkappa(const Universe& u, double l)
{
    const double wmax = u.w_z(5.);
    //dbg<<"wmax="<<wmax<<endl;

    IntRegion<double> intreg(0.,wmax);
    pkappaIntegrand integrand(u,l);
    //dbg<<"before integ for pkappa("<<l<<")"<<endl;
    double answer = int1d(
        integrand,intreg,1.e-15, u.precise ? 1.e-3 : 1.e-1);
    //dbg<<"pkappa("<<l<<") = "<<answer<<endl;
    return answer;
}

#define LO_ACC 1.e-2
#define MED_ACC 1.e-3
#define HI_ACC 1.e-6
#define SAVE_ORDER 8

Universe::Universe(double _om_m, double _om_v, double _om_b, double _h,
                   double _A, double _n, double _w, double _wa) :
    om_m(_om_m), om_v(_om_v), om_b(_om_b), h(_h), A_s(_A), n(_n),
    w(_w), wa(_wa), 
    coverH(2997.9/h), coverHcb(coverH*coverH*coverH), 
    pfint(0.), pbint(0.),
#ifndef SINGLEZB
    sg(bind21(ptr_fun(rawg),*this),HI_ACC,SAVE_ORDER), 
    spf(bind21(ptr_fun(rawpf),*this),HI_ACC,SAVE_ORDER), 
    spb(bind21(ptr_fun(rawpb),*this),HI_ACC,SAVE_ORDER), 
#endif
    sw_z(bind21(ptr_fun(raww_z),*this),HI_ACC,SAVE_ORDER),
    spom(bind21(ptr_fun(rawpom),*this),MED_ACC,SAVE_ORDER), 
    spgam(bind21(ptr_fun(rawpgam),*this),MED_ACC,SAVE_ORDER), 
    spkappa(bind21(ptr_fun(rawpkappa),*this),MED_ACC,SAVE_ORDER),
    sfom(bind21(ptr_fun(rawfom),*this),LO_ACC,SAVE_ORDER),
    sfgam(bind21(ptr_fun(rawfgam),*this),LO_ACC,SAVE_ORDER),
    precise(true), def(false)
{
    K = om_m+om_v-1.0;
    if (fabs(K) < 1.e-6) { 
        flat = true; 
        K = 0.; 
        sqrtK = 0.; 
    } else { 
        flat = false; 
        sqrtK = sqrt(fabs(K));
    }
}

Universe::Universe(const UParams& up) :
    om_m(up.GetOmM()), om_v(1.0-om_m), om_b(up.GetOmB()),
    h(up.h), A_s(up.As), n(up.n), w(up.w), wa(up.wa),
    coverH(2997.9/h), coverHcb(coverH*coverH*coverH), 
    K(0.0), flat(true), sqrtK(0.0),
    pfint(0.),pbint(0.),
#ifndef SINGLEZB
    sg(bind21(ptr_fun(rawg),*this),HI_ACC,SAVE_ORDER), 
    spf(bind21(ptr_fun(rawpf),*this),HI_ACC,SAVE_ORDER), 
    spb(bind21(ptr_fun(rawpb),*this),HI_ACC,SAVE_ORDER), 
#endif
    sw_z(bind21(ptr_fun(raww_z),*this),HI_ACC,SAVE_ORDER),
    spom(bind21(ptr_fun(rawpom),*this),MED_ACC,SAVE_ORDER), 
    spgam(bind21(ptr_fun(rawpgam),*this),MED_ACC,SAVE_ORDER), 
    spkappa(bind21(ptr_fun(rawpkappa),*this),MED_ACC,SAVE_ORDER),
    sfom(bind21(ptr_fun(rawfom),*this),LO_ACC,SAVE_ORDER),
    sfgam(bind21(ptr_fun(rawfgam),*this),LO_ACC,SAVE_ORDER),
    precise(true), def(false)
{ }

Universe::Universe(const UParamsNonFlat& up) :
    om_m(up.GetOmM()), om_v(up.omde), om_b(up.GetOmB()),
    h(up.h), A_s(up.As), n(up.n), w(up.w), wa(up.wa),
    coverH(2997.9/h), coverHcb(coverH*coverH*coverH), 
    pfint(0.),pbint(0.),
#ifndef SINGLEZB
    sg(bind21(ptr_fun(rawg),*this),HI_ACC,SAVE_ORDER), 
    spf(bind21(ptr_fun(rawpf),*this),HI_ACC,SAVE_ORDER), 
    spb(bind21(ptr_fun(rawpb),*this),HI_ACC,SAVE_ORDER), 
#endif
    sw_z(bind21(ptr_fun(raww_z),*this),HI_ACC,SAVE_ORDER),
    spom(bind21(ptr_fun(rawpom),*this),MED_ACC,SAVE_ORDER), 
    spgam(bind21(ptr_fun(rawpgam),*this),MED_ACC,SAVE_ORDER), 
    spkappa(bind21(ptr_fun(rawpkappa),*this),MED_ACC,SAVE_ORDER),
    sfom(bind21(ptr_fun(rawfom),*this),LO_ACC,SAVE_ORDER),
    sfgam(bind21(ptr_fun(rawfgam),*this),LO_ACC,SAVE_ORDER),
    precise(true), def(false)
{ 
    K = om_m+om_v-1.0;
    if (fabs(K) < 1.e-6) { 
        flat = true; 
        K = 0.; 
        sqrtK = 0.; 
    } else { 
        flat = false; 
        sqrtK = sqrt(fabs(K));
    }
}

// For all of these, k is in units of Mpc^-1 (not h/Mpc)
double Universe::NLDelta3d(double k, double z) const
{
    static bool first=true;
    if (!def) {
        def_universe_(om_m,om_v,om_b,h,A_s,n,w,wa); 
        def = true; 
    }
#ifdef PD
    if (first) {
        dbg<<"Using Peacock-Dodds Non-Linear model\n";
        first = false;
    }
    double p3d = pdp3d_(k,z); 
    //xdbg<<"p3d("<<k<<','<<z<<") = "<<p3d<<endl;
    return p3d;
#else
    if (first) {
        dbg<<"Using Smith Non-Linear model\n";
        first = false;
    }
    return smithp3d_(k,z); 
#endif
}

double Universe::NLP3d(double k, double z) const
{ return NLDelta3d(k,z) * TWOPISQ/(k*k*k); }

double Universe::LinearP3d(double k, double z) const
{ 
    if (!def) {
        def_universe_(om_m,om_v,om_b,h,A_s,n,w,wa); 
        def = true; 
    }
    return linearp3d_(k,z); 
}

double Universe::LinearDelta3d(double k, double z) const
{ return LinearP3d(k,z) * (k*k*k)/TWOPISQ; }

double Universe::fk(double w)  const
// fk(w) is the comoving angular diameter distance at comoving distance w
//
// fk(w) = w if K = 0
//       =  sqrt(K)^-1 sin( sqrt(K) w) if K > 0
//       = sqrt(-K)^-1 sin(sqrt(-K) w) if K < 0
//
// Note: we use the usual dimensionless version of w (= w*H0/c)
// which is approriate here.  The answer is also dimensionless (ie. in 
// unit of c/H0).
//
{
    if (flat) return w;
    else if (K > 0.) {
        //dbg<<"Warning: not flat\n";
        return sin(sqrtK*w)/sqrtK;
    } else {
        //dbg<<"Warning: not flat\n";
        return sinh(sqrtK*w)/sqrtK;
    }
}

double Universe::z_w(double w) const
// redshift as a function of coordinate distance (in units of c/Ho)
{
    const double dz = 0.05;
    typedef map<double,double>::iterator mapit;
    // for each element it->first = w, it->second = z

    if (zwmap.size() == 0) zwmap[0.] = 0.;

    Assert(w >= 0.);

    if (zwmap.rbegin()->first <= w) {
        for(double z1 = zwmap.rbegin()->second + dz; 
            zwmap.rbegin()->first <= w; z1 += dz) {
            zwmap[w_z(z1)] = z1;
        }
    }

    mapit it1 = zwmap.upper_bound(w);
    Assert(it1 != zwmap.end());
    Assert(it1 != zwmap.begin());
    Assert(it1->first > w);
    mapit it0 = it1; --it0;
    Assert(it0->first <= w);
    double w0 = it0->first;
    double w1 = it1->first;
    double z0 = it0->second;
    double z1 = it1->second;
    // (z-z0)/(z1-z0) = (w-w0)/(w1-w0)
    return (w-w0)/(w1-w0)*(z1-z0) + z0;
}

struct fnnIntegrand : public std::unary_function<double, double>
{
    fnnIntegrand(const Universe& _u, double _R, double (*_W)(double) ) : 
        u(_u), R(_R),W(_W) {}
    double operator()(double logl) const 
    {
        double l = exp(logl);
        return l * l * u.pom(l) * W(l*R);
    }

    const Universe& u;
    double R;
    double (*W)(double);
};

double Universe::fnn(double R) const
{
    const double factor = 1. / coverHcb / TWOPI;

    IntRegion<double> intreg(2.,15.);
    fnnIntegrand integrand(*this,R,&WMap);
    return int1d(
        integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1) * factor;
}

struct fnmIntegrand : public std::unary_function<double, double>
{
    fnmIntegrand(const Universe& _u, double _R, double (*_W)(double) ) :
        u(_u), R(_R),W(_W) {}
    double operator()(double logl) const 
    {
        double l = exp(logl);
        return l * l * u.pgam(l) * W(l*R);
    }

    const Universe& u;
    double R;
    double (*W)(double);
};

double Universe::fnm(double R) const
{
    const double factor = om_m * 1.5 / coverHcb / TWOPI;

    IntRegion<double> intreg(2.,15.);
    fnmIntegrand integrand(*this,R,&WMap);
    return int1d(
        integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1) * factor;
}

struct fmmIntegrand : public std::unary_function<double, double>
{
    fmmIntegrand(const Universe& _u, double _R, double (*_W)(double) ) :
        u(_u),R(_R),W(_W) {}
    double operator()(double logl) const 
    { 
        double l = exp(logl);
        //xdbg<<"fmminteg("<<logl<<") = "<<l<<"^2 * "<<u.pkappa(l)<<" * "<<W(l*R)<<endl;
        return l * l * u.pkappa(l) * W(l*R); 
    }

    const Universe& u;
    double R;
    double (*W)(double);
};

double Universe::fmm(double R) const
{
    const double factor = SQR(om_m*1.5) / coverHcb / TWOPI;

    IntRegion<double> intreg(2.,15.);
    fmmIntegrand integrand(*this,R,&WMap);
    //xdbg<<"before integ for fmm("<<R<<")"<<endl;
    double answer = int1d(
        integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1) * factor;
    //xdbg<<"fmm("<<R<<") = "<<answer<<endl;
    return answer;
}

double Universe::fvar(double R) const
{
    const double factor = SQR(om_m*1.5) / coverHcb / TWOPI;

    IntRegion<double> intreg(2.,15.);
    fmmIntegrand integrand(*this,R,&WVar);
    return int1d(
        integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1) * factor;
}

struct altfnmIntegrand : public std::unary_function<double, double>
{
    altfnmIntegrand(const Universe& _u, double _R) :
        u(_u), R(_R), Rsq(_R*_R) {}
    double operator()(double r) const
    { return r/Rsq * u.fgam(r) * Tcross(r/R); }

    const Universe& u;
    double R,Rsq;
};

double Universe::altfnm(double R) const
// fnm = int dr r/R^2 fgam(r) Tcross(r/R)
{
    //dbg<<"start altfnm"<<endl;
    IntRegion<double> intreg(0.,20.*R);
    altfnmIntegrand integrand(*this,R);
    //dbg<<"before integrate"<<endl;
    double answer = int1d(
        integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1);
    //dbg<<"answer = "<<answer<<endl;
    return answer;
}

struct altfnnIntegrand : public std::unary_function<double, double>
{
    altfnnIntegrand(const Universe& _u, double _R) :
        u(_u), R(_R), Rsq(_R*_R) {}
    double operator()(double r) const
    { return r/Rsq * u.fom(r) * Tplus(r/R); }

    const Universe& u;
    double R,Rsq;
};

double Universe::altfnn(double R) const
// fnn = int dr r/R^2 fom(r) Tcross(r/R)
{
    IntRegion<double> intreg(0.,20.*R);
    altfnnIntegrand integrand(*this,R);
    double answer = int1d(
        integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1);
    return answer;
}

double Wsigma(double x) 
    // Wsigma(x) = 3x^-3(sin x - x cos x)
    // = the fourier transform of a spherical top hat window
{ return (sin(x)-x*cos(x))*3/(x*x*x); }

struct sigIntegrand : public std::unary_function<double, double>
{
    sigIntegrand(const Universe& _u,double _r) : u(_u),r(_r) {}
    double operator()(double lnk) const 
    { 
        double k = exp(lnk);
        return SQR(Wsigma(k*r))*u.LinearDelta3d(k,0.); 
    }

    const Universe& u;
    double r;
};

double Universe::sigma(double r) const
// sigma(r)^2 = int d^3k/(2Pi)^3 P(k,0) Wsigma^2(kr)
{
    // input r is in h^-1 Mpc
    double roh = r/h;

    IntRegion<double> intreg(-10.,10.);
    sigIntegrand integrand(*this,roh);

    double sig = sqrt(int1d(
            integrand,intreg,1.e-15, precise ? 1.e-3 : 1.e-1));
    //dbg<<"Sigma(int1d) = "<<sig<<endl;
    return sig;
}

